package org.maxgroundstation.simulation;

import java.util.Iterator;

import org.maxgroundstation.simulation.service.Service;
import org.maxgroundstation.simulation.service.video.VideoService;


/**
 * A static utility class for parsing robot commands.
 * @author Scott Davis
 * 
 * Copyright (C) 2010  Scott Davis
 * 
 * This program is free software: you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation, either version 3 of the License, or
 * (at your option) any later version.
 * 
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 * 
 * You should have received a copy of the GNU General Public License
 * along with this program.  If not, see <http://www.gnu.org/licenses/>.
 */
class CommandParser {

    // Command response codes.
    public static final int SUCCESS_REPONSE = 0;
    public static final int PARSE_ERROR_RESPONSE = 1;
    public static final int RANGE_ERROR_RESPONSE = 2;
    
    /**
     * Private constructor for static utility class.
     */
    private CommandParser() {};
    
    /**
     * Send a formatted command to the robot.
     * Ex: motor(1.0)
     * Ex: turn(.3)
     * Ex: turnfront(.7)
     * Ex: turnrear(-.5)
     * Ex: pan(-1.0)
     * Ex: tilt(-.5)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    static int parseCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        
        if ((commandString != null)) {
            if (commandString.startsWith("motor(")) {
                result = parseMotorCommand(robot, commandString);
            }
            else if (commandString.startsWith("turn(")) {
                result = parseTurnCommand(robot, commandString);
            }
            else if (commandString.startsWith("turnfront(")) {
                result = parseTurnFrontCommand(robot, commandString);
            }
            else if (commandString.startsWith("turnrear(")) {
                result = parseTurnRearCommand(robot, commandString);
            }
            else if (commandString.startsWith("pan(")) {
                result = parsePanCommand(robot, commandString);
            }
            else if (commandString.startsWith("tilt(")) {
                result = parseTiltCommand(robot, commandString);
            }
            else if (commandString.startsWith("imalive(")) {
                result = parseImAliveCommand(robot, commandString);
            }
        }
        
        return result;
    }
    
    /**
     * Send a formatted text command to the robot.
     * Ex: "videoparams:320,240,30"
     * 
     * @param robot the MAX robot.
     * @param commandText the formatted text command.
     * @return response code.
     */
    static int parseTextCommand(MaxRobot robot, String commandText) {
        int result = PARSE_ERROR_RESPONSE;
        
        if (commandText != null) {
            int colonIndex = commandText.indexOf(":");
            if (colonIndex > 0) {
                String commandName = commandText.substring(0, colonIndex);
                String textValue = commandText.substring(colonIndex + 1);
                
                // Set video parameters.
                if (commandName.equals("videoparams")) {
                    String[] params = textValue.split(",");
                    if (params.length == 3) {
                        int width = Integer.parseInt(params[0]);
                        int height = Integer.parseInt(params[1]);
                        int fps = Integer.parseInt(params[2]);
                        
                        // Set the video parameters in all video services.
                        Iterator<Service> i = robot.getServiceCommunication().getServices().iterator();
                        while (i.hasNext()) {
                        	Service service = i.next();
                        	if (service instanceof VideoService) {
                        		VideoService videoService = (VideoService) service;
                        		videoService.setVideoImageWidth(width);
                        		videoService.setVideoImageHeight(height);
                        		videoService.setFPS(fps);
                        	}
                        }
                    }
                }
            }
        }
        
        return result;
    }
    
    /**
     * Parse the motor command.
     * Ex: motor(.5)
     * @param robot the MAX robot
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parseMotorCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "motor(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = Float.parseFloat(valueStr);
                boolean inFrontAxleRange = ((value >= MaxRobot.MIN_FRONT_AXLE_MOTOR_LIMIT) && 
                        (value <= MaxRobot.MAX_FRONT_AXLE_MOTOR_LIMIT));
                boolean inRearAxleRange = ((value >= MaxRobot.MIN_REAR_AXLE_MOTOR_LIMIT) && 
                        (value <= MaxRobot.MAX_REAR_AXLE_MOTOR_LIMIT));
                if (inFrontAxleRange && inRearAxleRange) {
                    robot.setFrontAxleMotor(value);
                    robot.setRearAxleMotor(value);
                    result = SUCCESS_REPONSE;
                }
                else result = RANGE_ERROR_RESPONSE;
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the turn command (turn both wheels with anti-roll feature).
     * Ex: turnfront(.3)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parseTurnCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "turn(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = Float.parseFloat(valueStr);
                boolean inFrontWheelsRange = ((value >= MaxRobot.MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT) && 
                        (value <= MaxRobot.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT));
                boolean inRearWheelsRange = ((value <= (0F - MaxRobot.MIN_REAR_WHEELS_TURN_ANGLE_LIMIT)) && 
                        (value >= (0F - MaxRobot.MAX_REAR_WHEELS_TURN_ANGLE_LIMIT)));
                if (inFrontWheelsRange && inRearWheelsRange) {
                    robot.setAntiRollWheelsTurnAngle(value);
                    result = SUCCESS_REPONSE;
                }
                else result = RANGE_ERROR_RESPONSE;
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the turnfront command.
     * Ex: turnfront(.7)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parseTurnFrontCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "turnfront(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = Float.parseFloat(valueStr);
                boolean inFrontWheelsRange = ((value >= MaxRobot.MIN_FRONT_WHEELS_TURN_ANGLE_LIMIT) && 
                        (value <= MaxRobot.MAX_FRONT_WHEELS_TURN_ANGLE_LIMIT));
                if (inFrontWheelsRange) {
                    robot.setFrontWheelsTurnAngle(value);
                    result = SUCCESS_REPONSE;
                }
                else result = RANGE_ERROR_RESPONSE;
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the turnrear command.
     * Ex: turnrear(-.5)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parseTurnRearCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "turnrear(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = 0F - Float.parseFloat(valueStr);
                boolean inRearWheelsRange = ((value >= MaxRobot.MIN_REAR_WHEELS_TURN_ANGLE_LIMIT) && 
                        (value <= MaxRobot.MAX_REAR_WHEELS_TURN_ANGLE_LIMIT));
                if (inRearWheelsRange) {
                    robot.setRearWheelsTurnAngle(value);
                    result = SUCCESS_REPONSE;
                }
                else result = RANGE_ERROR_RESPONSE;
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the pan command.
     * Ex: pan(-1.0)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parsePanCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "pan(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = Float.parseFloat(valueStr);
                /*
                boolean inHeadPanRange = ((value >= MaxRobot.MIN_HEAD_PAN_ANGLE) && 
                        (value <= MaxRobot.MAX_HEAD_PAN_ANGLE));
                if (inHeadPanRange) {
                */
                    robot.setHeadPanAngle(value);
                    result = SUCCESS_REPONSE;
                    /*
                }
                else result = RANGE_ERROR_RESPONSE;
                */
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the tilt command.
     * tilt(-.5)
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return response code.
     */
    private static int parseTiltCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        int prefixLength = "tilt(".length();
        int suffixIndex = commandString.indexOf(")");
        if (suffixIndex > prefixLength) {
            String valueStr = commandString.substring(prefixLength, suffixIndex);
            try {
                float value = Float.parseFloat(valueStr);
                boolean inHeadTiltRange = ((value >= MaxRobot.MIN_HEAD_TILT_ANGLE) && 
                        (value <= MaxRobot.MAX_HEAD_TILT_ANGLE));
                if (inHeadTiltRange) {
                    robot.setHeadTiltAngle(value);
                    result = SUCCESS_REPONSE;
                }
                else result = RANGE_ERROR_RESPONSE;
            }
            catch (Exception e) {}
        }
        return result;
    }
    
    /**
     * Parse the imalive command.
     * @param robot the MAX robot.
     * @param commandString the formatted command string.
     * @return the response code.
     */
    private static int parseImAliveCommand(MaxRobot robot, String commandString) {
        int result = PARSE_ERROR_RESPONSE;
        
        try {
            String ipAddress = robot.getUdpCommandReceiver().getLastDatagramAddress();
            //int port = UdpImageSender.DEFAULT_IMAGE_RECEIVER_PORT;
            if (ipAddress != null) {
                //robot.getImageSender().renewConnection(ipAddress, port);
                result = SUCCESS_REPONSE;
            }
        }
        catch (Exception e) {}
        
        return result;
    }
}